//
//  LinAlg.c
//  Vector_Math
//
//  Created by John R Peterson on 3/10/12.
//  Copyright 2012 All rights reserved.
//

#include "LinAlg.h"

/*
 * Note that I at then end of an operation means inplace, as in one of the arguments is modified
 * these inplace operations return the target for convenience
 *
 * Other functions do not modify input arguments and instead return a new struct
 *
 * Most functions if given incompatible vectors ie dimensions don't match up will exit the program
 * equals is the exception and will merely return false
 *
 * To use existing arrays without copying the array with Vector math simply use Vec_createMan, but be careful
 */

// sums two vectors and returns a new vector
Vec* Vec_add(Vec *a, Vec *b) {
    Vec *c = Vec_copy(a);
    return Vec_addI(c,b);
}

// sums in place modifying target
Vec* Vec_addI(Vec *target, Vec *b) {
    if ((target->size != b->size) || (target->isColumn != b->isColumn)) {
        printf("Warning: incompatible dimensions Vec_addI");
        exit(1);
    }
    
    int i;
    for (i = 0; i < target->size; i++) {
        target->val[i] += b->val[i];
    }
    return target;
}

Vec* Vec_addID(Vec *target, Vec *b) {
    Vec_addI(target,b);
    Vec_destroy(b);
    return target;
}

Vec* Vec_addD(Vec *a, Vec *b) {
    Vec *ans = Vec_add(a,b);
    Vec_destroy(a);
    return ans;
}

Vec* Vec_add_D(Vec *a, Vec *b) {
    Vec *ans = Vec_add(a,b);
    Vec_destroy(b);
    return ans;
}

Vec* Vec_addDD(Vec *a, Vec *b) {
    Vec *ans = Vec_add(a,b);
    Vec_destroy(a);
    Vec_destroy(b);
    return ans;
}


// convolution of discrete valued function in 1D
// returns a new discrete function with same length as first argument 
// need the kernel to be "odd" so that don't shift anything
Vec* Vec_convolveC(Vec* v, Vec *k) {
    if ((k->size)%2 == 0) {
        printf("Warning: expected odd kernel Vec_convolve");
        exit(1);
    }
    
    Vec* ans = Vec_createE(v->size);
    int start, end, i, j;
    int klength = k->size;
    int vlength = v->size;
    int klength2 = klength / 2;
    
    for (i = 0; i < vlength; i++) {
        // lower bound
        if ((i - klength2) < 0) {
            start = klength2 - i; // need to start later to stay in bounds
        } else {
            start = 0;
        }
        // upper bound
        if ((i + klength2) >= vlength) {
            end = vlength - i + klength2; // need to stop sooner to stay in bounds
        } else {
            end = klength;
        }
        
        // zero out
        ans->val[i] = 0;
        for (j = start; j < end; j++) {
            ans->val[i] += v->val[i + j - klength2] * k->val[(klength - 1) - j]; // sum across elements have to reflect the kernel
        }
    }
    return ans;
    
}

// finds the maximum of the product of the vector with the kernel
Vec* Vec_maxConvolveC(Vec *v, Vec *k) {
    if ((k->size)%2 == 0) {
        printf("Warning: expected odd kernel Vec_convolve");
        exit(1);
    }
    
    Vec* ans = Vec_createE(v->size);
    int start, end, i, j;
    float max, result;
    int klength = k->size;
    int vlength = v->size;
    int klength2 = klength / 2;
    
    for (i = 0; i < vlength; i++) {
        // lower bound
        if ((i - klength2) < 0) {
            start = klength2 - i; // need to start later to stay in bounds
        } else {
            start = 0;
        }
        // upper bound
        if ((i + klength2) >= vlength) {
            end = vlength - i + klength2; // need to stop sooner to stay in bounds
        } else {
            end = klength;
        }
        
        // find maximum in region
        max = 0;
        for (j = start; j < end; j++) {
            result = v->val[i + j - klength2] * k->val[(klength - 1) - j]; // sum across elements have to reflect the kernel
            if (result > max) {
                max = result;
            }
        }
        // write maximum
        ans->val[i] = max;
    }
    return ans;
}

Vec* Vec_copy(Vec *v) {
    Vec *newV = Vec_createE(v->size);
    memcpy(newV->val,v->val,sizeof(float)*(v->size));
    newV->isColumn = v->isColumn;
    return newV;
}

// automactically allocates and copies array over
Vec* Vec_create(float values[], int size) {
    Vec *v = Vec_createE(size);
    memcpy(v->val,values,sizeof(float)*size);
    return v;
}

// allocates but does not initialize values field
Vec* Vec_createE(int size) {
    if (size <= 0) {
        printf("Warning: size must be atleast 1 in Vec_createE\n");
        exit(1);
    }
    
    // allocate space for the struct
    Vec *v = (Vec *) malloc(sizeof(Vec));
    if (v == NULL) {
        printf("Warning: malloc failed in Vec_CreateE:1\n");
        exit(1);
    }
    
    // allocate space for a copy of the array
    v->val = (float *) malloc(sizeof(float)*size);
    if (v->val == NULL) {
        printf("Warning: malloc failed in Vec_CreateE:2\n");
        exit(1);
    }
    
    v->size = size;
    v->mode = TRUE;
    v->isColumn = TRUE;
    return v;
}

Vec* Vec_createGaussian(float sigma, int size) {
    Vec *G = Vec_createE(size);
    
    float x;
    float total = 0;
    int i;
    float offset = size/2.0 - 0.5;
    float sigma2 = sigma*sigma;
    float sf = 1/sqrt(2*PI*sigma2);
    for (i = 0; i < size; i++) {
        x = i - offset; // x value at the center of the cell
        G->val[i] = sf*exp((-x*x)/(2*sigma2));
        total += G->val[i];
    }
    G = Vec_scaleI(G,1/total); // normalize gaussian
    return G;
}

// does not copy the array
Vec* Vec_createMan(float values[], int size) {
    Vec *v = Vec_createE(size);
    v->val = values;
    v->mode = FALSE; // so that when we destroy it, don't free the array
    return v;
}

Vec* Vec_createZeros(int size) {
    Vec *v = Vec_createE(size);
    int i;
    for (i = 0; i < size; i++) {
        v->val[i] = 0;
    }
    return v;
}

Vec* Vec_crossProduct(Vec *a, Vec*b) {
    if (a->size != b->size) {
        printf("Warning: incompatible dimensions Vec_crossProduct");
        exit(1);
    }
    
    if ((a->size != 3) && (a->size != 4)) {
        printf("Warning: cross product only supports 3D or 3D homogeneous coordinates");
        exit(1);
    }
    
    Vec *ans = Vec_createE(a->size);
    
    // set homogeneous coordinate
    if (a->size == 4) {
        ans->val[3] = 1;
    }
    
    ans->val[0] = a->val[1] * b->val[2] - a->val[2] * b->val[1];
    ans->val[1] = a->val[2] * b->val[0] - a->val[0] * b->val[2];
    ans->val[2] = a->val[0] * b->val[1] - a->val[1] * b->val[0];
    
    return ans;
}

Vec* Vec_crossProductD(Vec *a, Vec*b) {
    Vec *ans = Vec_crossProduct(a, b);
    Vec_destroy(a);
    return ans;
}

Vec* Vec_crossProduct_D(Vec *a, Vec*b) {
    Vec *ans = Vec_crossProduct(a, b);
    Vec_destroy(b);
    return ans;
}
Vec* Vec_crossProductDD(Vec *a, Vec*b) {
    Vec *ans = Vec_crossProduct(a, b);
    Vec_destroy(a);
    Vec_destroy(b);
    return ans;
}

void Vec_destroy(Vec *target) {
    // if we are managing it, then we should destroy that memory aswell
    if(target->mode) {
        free(target->val);
    }
    free(target);
}

float Vec_dotProduct(Vec *a, Vec *b) {
    if (a->size != b->size) {
        printf("Warning: incompatible dimensions Vec_dotProduct");
        exit(1);
    }
    
    float ans = 0;
    int i;
    for (i = 0; i < a->size; i++) {
        ans += (a->val[i])*(b->val[i]);
    }
    return ans;
}

float Vec_dotProductD(Vec *a, Vec *b) {
    float ans = Vec_dotProduct(a, b);
    Vec_destroy(a);
    return ans;
}

float Vec_dotProduct_D(Vec *a, Vec *b) {
    float ans = Vec_dotProduct(a, b);
    Vec_destroy(b);
    return ans;
}

float Vec_dotProductDD(Vec *a, Vec *b) {
    float ans = Vec_dotProduct(a, b);
    Vec_destroy(a);
    Vec_destroy(b);
    return ans;
}

short Vec_equals(Vec *a, Vec *b) {
    if ((a == NULL) || (b == NULL) || (a->size != b->size) || (a->isColumn != b->isColumn)) {
        return FALSE;
    }
    int i;
    for (i = 0; i < a->size; i++) {
        if (a->val[i] != b->val[i]) {
            return FALSE;
        }
    }
    return TRUE;
}

// L2 norm
float Vec_L2norm(Vec *v) {
    float sum = 0;
    int i;
    for (i = 0; i < v->size; i++) {
        sum += (v->val[i])*(v->val[i]);
    }
    return sqrtf(sum);
}

float Vec_L2normD(Vec *v) {
    float ans = Vec_L2norm(v);
    Vec_destroy(v);
    return ans;
}

// squared L2 norm
float Vec_L2norm2(Vec *v) {
    float sum = 0;
    int i;
    for (i = 0; i < v->size; i++) {
        sum += (v->val[i])*(v->val[i]);
    }
    return sum;
}

Matrix* Vec_multiply(Vec *a, Vec *b) {
    if ((a->isColumn == b->isColumn) && (a->size != 1) && (b->size != 1)) {
        printf("Warning: incompatible dimensions: Vec_multiply\n");
        exit(1);
    }
    
    // case where one has length 1
    if (a->size == 1) {
        return Vec_toMatrix(Vec_scale(b,a->val[0]));
    } else if (b->size == 1) {
        return Vec_toMatrix(Vec_scale(a,b->val[0]));
    } 
    // handle dot product like case
    else if (b->isColumn) {
        float val = Vec_dotProduct(a,b);
        return Matrix_createV(&val,1,1);
    }
    // handle full on matrix case
    else {
        Matrix *m = Matrix_createE(a->size,b->size);
        int i,j;
        for (i = 0; i < m->rows; i++) {
            for (j = 0; j < m->columns; j++) {
                m->val[i][j] = (a->val[i])*(b->val[j]);  
            }
        }
        return m;
    }
}

Matrix* Vec_multiplyD(Vec *a, Vec *b) {
    Matrix *ans = Vec_multiply(a, b);
    Vec_destroy(a);
    return ans;
}

Matrix* Vec_multiply_D(Vec *a, Vec *b) {
    Matrix *ans = Vec_multiply(a, b);
    Vec_destroy(b);
    return ans;
}

Matrix* Vec_multiplyDD(Vec *a, Vec *b) {
    Matrix *ans = Vec_multiply(a, b);
    Vec_destroy(a);
    Vec_destroy(b);
    return ans;
}


Vec* Vec_normalize(Vec *v) {
    float norm = Vec_L2norm(v);
    return Vec_scale(v,1/norm);
}

Vec* Vec_normalizeI(Vec *target) {
    float norm = Vec_L2norm(target);
    return Vec_scaleI(target, 1/norm);
}

void Vec_print(Vec *v) {
    // 15 is the minimal size needed to hold the buffer with the below formatting specifications
    char full[(v->size)*15];
    char buffer[15];
    strcpy(full,""); // need a null termination to make strcat happy
    int i;
    float f;
    for (i = 0; i < v->size; i++) {
        f = v->val[i];
        if (f >= 1000000) {
            sprintf(buffer,"%.5e ",f);
        } else {
            sprintf(buffer,"%.5f ",f);
        }
        strcat(full,buffer);
    }
    if (v->isColumn) {
        printf(" [ %s] T\n",full);
    } else {
        printf("[ %s]\n",full);
    }
}

Vec* Vec_scale(Vec *v, float scaleFactor) {
    Vec *ans = Vec_copy(v);
    return Vec_scaleI(ans,scaleFactor);
}

Vec* Vec_scaleI(Vec *target, float scaleFactor) {
    int i;
    for (i = 0; i < target->size; i++) {
        target->val[i] *= scaleFactor;
    }
    return target;
}

// subtracts two vectors and returns a new vector
Vec* Vec_subtract(Vec *a, Vec *b) {
    Vec *c = Vec_copy(a);
    return Vec_subtractI(c,b);
}

// subtracts in place modifying target
Vec* Vec_subtractI(Vec *target, Vec *b) {
    if ((target->size != b->size) || (target->isColumn != b->isColumn)) {
        printf("Warning: incompatible dimensions Vec_subtractI");
        exit(1);
    }
    
    int i;
    for (i = 0; i < target->size; i++) {
        target->val[i] -= b->val[i];
    }
    return target;
}

Vec* Vec_subtractID(Vec *target, Vec *b) {
    Vec_subtractI(target, b);
    Vec_destroy(b);
    return target;
}

Vec* Vec_subtractD(Vec *a, Vec *b) {
    Vec *ans = Vec_subtract(a, b);
    Vec_destroy(a);
    return ans;
}

Vec* Vec_subtract_D(Vec *a, Vec *b) {
    Vec *ans = Vec_subtract(a, b);
    Vec_destroy(b);
    return ans;
}

Vec* Vec_subtractDD(Vec *a, Vec *b) {
    Vec *ans = Vec_subtract(a, b);
    Vec_destroy(a);
    Vec_destroy(b);
    return ans;
}

Matrix* Vec_toMatrix(Vec *v) {
    Matrix *m;
    if (v->isColumn) {
        m = Matrix_createV(v->val,1,v->size);
    } else {
        m = Matrix_createV(v->val,v->size,1);
    }
    return m;
}

// actually just adds a 0 component
Vec* Vec_2Dto3DI(Vec *target) {
    
    target->val = (float *) realloc(target->val, sizeof(float) * (target->size + 1));
    if (target->val == NULL) {
        printf("Warning: realloc failed in Vec_toHI\n");
        exit(1);
    }
    target->val[target->size] = 0.0;
    (target->size)++;
    
    return target;
}

// actually just chops off a component
Vec* Vec_3Dto2DI(Vec *target) {
    if (target->size > 1) {
        // shrink the vector to its new size
        target->val = (float *) realloc(target->val, sizeof(float) * (target->size - 1));
        if (target->val == NULL) {
            printf("Warning: realloc failed in Vec_RHI\n");
            exit(1);
        }
        (target->size)--;
    } else {
        printf("Warning: vector must have more than a single coordinate Vec_RHI\n");
        exit(1);
    }
    return target;
}

// converts a vector to homogeneous coordinates in place
Vec* Vec_toHI(Vec *target) {
    target->val = (float *) realloc(target->val, sizeof(float) * (target->size + 1));
    if (target->val == NULL) {
        printf("Warning: realloc failed in Vec_toHI\n");
        exit(1);
    }
    target->val[target->size] = 1.0;
    (target->size)++;
    
    return target;
}

// transforms vector in homogeneous coordinates back to normal in place
// checks to make sure homogenous coordinate is 1
Vec* Vec_RHI(Vec *target) {
    if (target->size > 1) {
        // if homogeneous coordinate is not already 1 then scale the vector
        if (target->val[target->size - 1] != 1) {
            Vec_scaleI(target, 1/(target->val[target->size -1]));
        }
        // shrink the vector to its new size
        target->val = (float *) realloc(target->val, sizeof(float) * (target->size - 1));
        if (target->val == NULL) {
            printf("Warning: realloc failed in Vec_RHI\n");
            exit(1);
        }
        (target->size)--;
    } else {
        printf("Warning: vector must have more than a single coordinate Vec_RHI\n");
        exit(1);
    }
    return target;
}

Vec* Vec_transpose(Vec *v) {
    Vec *newV = Vec_copy(v);
    return Vec_transposeI(newV);
}

Vec* Vec_transposeI(Vec *target) {
    target->isColumn = !(target->isColumn);
    return target;
}

/***************************************************************************/

Matrix* Matrix_add(Matrix *A, Matrix *B) {
    Matrix *C = Matrix_copy(A);
    return Matrix_addI(C,B);
}

Matrix* Matrix_addI(Matrix *target, Matrix *B) {
    if ((target->rows != B->rows) || (target->columns != B->columns)) {
        printf("Warning: Incompatible dimensions: Matrix_addI\n");
        exit(1);
    }
    
    int i,j;
    for (i = 0; i < target->rows; i++) {
        for (j = 0; j < target->columns; j++) {
            target->val[i][j] += B->val[i][j];
        }
    }
    return target;
}

Matrix* Matrix_addID(Matrix *target, Matrix *B) {
    Matrix_addI(target, B);
    Matrix_destroy(B);
    return target;
}

Matrix* Matrix_addD(Matrix *A, Matrix *B) {
    Matrix *ans = Matrix_add(A, B);
    Matrix_destroy(A);
    return ans;
}

Matrix* Matrix_add_D(Matrix *A, Matrix *B) {
    Matrix *ans = Matrix_add(A, B);
    Matrix_destroy(B);
    return ans;
}

Matrix* Matrix_addDD(Matrix *A, Matrix *B) {
    Matrix *ans = Matrix_add(A, B);
    Matrix_destroy(A);
    Matrix_destroy(B);
    return ans;
}

// 2D convolution, crops to size of F, K is the Kernel
// expects and odd kernel
Matrix* Matrix_convolveC(Matrix *F, Matrix *K) {
    
    if ((K->rows % 2 == 0) || (K->columns % 2 == 0)) {
        printf("Warning: expected odd kernel: Matrix_convolveC\n");
        exit(1);
    }
    
    Matrix* G = Matrix_createE(F->rows, F->columns); 
    
    int GWidth = G->rows; // x dimension, how many columns
    int GHeight = G->columns; // y dimension, how many rows
    int KWidth = K->rows;
    int KWidth2 = KWidth / 2;
    int KHeight = K->columns;
    int KHeight2 = KHeight / 2;
    
    int xstart, xend, ystart, yend;
    int i,j,x,y;
    for (i = 0; i < GWidth; i++) {
        for (j = 0; j < GHeight; j++) {
            // lower bound x
            if ((i - KWidth2) < 0) {
                xstart = KWidth2 - i;
            } else {
                xstart = 0;
            }
            // upper bound x
            if ((i + KWidth2) >= GWidth) {
                xend = GWidth - i + KWidth2;
            } else {
                xend = KWidth;
            }
            
            // lower bound y
            if ((j - KHeight2) < 0) {
                ystart = KHeight2 - j;
            } else {
                ystart = 0;
            }
            // upper bound y
            if ((j + KHeight2) >= GHeight) {
                yend = GHeight - j + KHeight2;
            } else {
                yend = KHeight;
            }
            // zero out element
            G->val[i][j] = 0;
            for (x = xstart; x < xend; x++) {
                for (y = ystart; y < yend; y++) {
                    G->val[i][j] += F->val[i + x - KWidth2][j + y - KHeight2] * K->val[(KWidth - 1) - x][(KHeight - 1) - y]; // reflect the kernel
                }
            }
        }
    }
    
    return G;

}

// max convolution, must be used with the appropriate kernel
Matrix* Matrix_maxConvolveC(Matrix *F, Matrix *K) {
    
    if ((K->rows % 2 == 0) || (K->columns % 2 == 0)) {
        printf("Warning: expected odd kernel: Matrix_convolveC\n");
        exit(1);
    }
    
    Matrix* G = Matrix_createE(F->rows, F->columns); 
    
    int GWidth = G->rows; // x dimension, how many columns
    int GHeight = G->columns; // y dimension, how many rows
    int KWidth = K->rows;
    int KWidth2 = KWidth / 2;
    int KHeight = K->columns;
    int KHeight2 = KHeight / 2;
    
    int xstart, xend, ystart, yend;
    int i,j,x,y;
    float max, result;
    for (i = 0; i < GWidth; i++) {
        for (j = 0; j < GHeight; j++) {
            // lower bound x
            if ((i - KWidth2) < 0) {
                xstart = KWidth2 - i;
            } else {
                xstart = 0;
            }
            // upper bound x
            if ((i + KWidth2) >= GWidth) {
                xend = GWidth - i + KWidth2;
            } else {
                xend = KWidth;
            }
            
            // lower bound y
            if ((j - KHeight2) < 0) {
                ystart = KHeight2 - j;
            } else {
                ystart = 0;
            }
            // upper bound y
            if ((j + KHeight2) >= GHeight) {
                yend = GHeight - j + KHeight2;
            } else {
                yend = KHeight;
            }
            
            // find maximum value
            max = 0;
            for (x = xstart; x < xend; x++) {
                for (y = ystart; y < yend; y++) {
                    result = F->val[i + x - KWidth2][j + y - KHeight2] * K->val[(KWidth - 1) - x][(KHeight - 1) - y]; // reflect the kernel
                    if (result > max) {
                        max = result;
                    }
                }
            }
            // write max
            G->val[i][j] = max;
        }
    }
    
    return G;
    
}


Matrix* Matrix_copy(Matrix *m) {
    Matrix *newM = Matrix_createE(m->rows,m->columns);
    memcpy(newM->val,m->val,sizeof(float) * (m->rows) * (m->columns));
    return newM;
}

// creates a matrix from a single vector assumed to be laid out as [row1, row2, row3...]
Matrix* Matrix_createV(float* values, int rows, int columns) {
    Matrix *m = Matrix_createE(rows, columns);
    memcpy(&(m->val[0][0]),values,sizeof(float) * rows * columns); // copy the input vector over to the address of the first element
    return m;
}

// use variable length arguments to add multiple 1D arrays to form a matrix
// expects # of rows comma seperated float *s of length columns
Matrix* Matrix_create(int rows, int columns, ...) {
    Matrix *m = Matrix_createE(rows, columns);
    va_list argP;
    va_start(argP,columns); // set up
    
    int i;
    for (i = 0; i < rows; i++) {
        memcpy(m->val[i],va_arg(argP,float *),sizeof(float)*columns); // for each argument, copy the array over
    }
    va_end(argP); // clean up
    return m;
}

// creates a matrix of dimensions about r^2 by r^2 with a circle of ones in within the circle and
// zero beyond it
// whether a cell is occupied or not is measured from its center, not striclty conservative
// always constructs odd kernels
Matrix* Matrix_createCircle(float radius) {
    int rows = 1 + 2*floorf(radius);
    Matrix *C = Matrix_createE(rows, rows);
    
    float offset =rows/2.0 - 0.5;
    
    int i,j;
    float x,x2,y,d; // where d measures the distance from the center
    for (i = 0; i < rows; i++) {
        x = i - offset;
        x2 = x*x;
        for (j = 0; j < rows; j++) {
            y = j - offset;
            d = sqrt(x2 + y*y);
            // if within the circle
            if (d <= radius) {
                C->val[i][j] = 1;
            } else {
                C->val[i][j] = 0;
            }
        }
    }
    
    return C;
}

Matrix* Matrix_createDiag(float *diag, int rows, int columns) {
    Matrix *D = Matrix_createE(rows,columns);
    int i, j, k = 0;
    for (i = 0; i < D->rows; i++) {
        for (j = 0; j < D->columns; j++) {
            if (i == j) {
                D->val[i][j] = diag[k];
                k++;
            } else {
                D->val[i][j] = 0;
            }
        }
    }
    return D;
}

// creates a rigid body transformation matrix that rotates by theta and then
// translates the resulting point by [dx, dy]
Matrix* Matrix_createRBT2D(float dx, float dy, float dtheta) {
    Matrix* rbt = Matrix_createE(3,3);
    // rotation part
    rbt->val[0][0] = cosf(dtheta);
    rbt->val[0][1] = -sinf(dtheta);
    rbt->val[1][0] = -(rbt->val[0][1]);
    rbt->val[1][1] = rbt->val[0][0];
    // translation
    rbt->val[0][2] = dx;
    rbt->val[1][2] = dy;
    // wrap up the last row
    rbt->val[2][0] = 0;
    rbt->val[2][1] = 0;
    rbt->val[2][2] = 1;
    return rbt;
}

/* creates a matrix of dimensions row x column with uninitialized elements */
Matrix* Matrix_createE(int rows, int columns) {
    if ((rows <= 0) || (columns <= 0)) {
        printf("Warning: invalid matrix dimensions in Matix create\n");
        exit(1);
    }
    
    // allocate space for the struct
    Matrix *m = (Matrix *) malloc(sizeof(Matrix));
    if (m == NULL) {
        printf("Warning: malloc failed in Matrix_createE:1\n");
        exit(1);
    }
    
    // allocate space for intermediate pointers
    m->val = (float **) malloc(sizeof(float *) * rows);
    if (m->val == NULL) {
        printf("Warning: malloc failed in Matrix_createE:2\n");
        exit(1);
    }
    
    // grab one big block
    float *temp = (float *) malloc(sizeof(float) * columns * rows);
    if (temp == NULL) {
        printf("Warning: malloc failed in Matrix_createE:3\n");
        exit(1);
    }
    
    // set intermediate pointers
    int i;
    for (i = 0; i < rows; i++) {
        m->val[i] = &(temp[i*columns]);
    }
    
    m->rows = rows;
    m->columns = columns;
    m->mode = TRUE;
    return m;
}

// creates 2D gaussian kernel of size x size dimensions
Matrix* Matrix_createGaussian(float sigma, int size) {
    Matrix *G = Matrix_createE(size,size);
    
    float x,y;
    float total = 0;
    int i, j;
    float offset = size/2.0 - 0.5;
    float sigma2 = sigma*sigma;
    float sf = 1/sqrt(2*PI*sigma2);
    for (i = 0; i < size; i++) {
        x = i - offset; // x value at the center of the cell
        for (j = 0; j < size; j++) {
            y = j - offset; // y value at the center of the cell
            G->val[i][j] = sf*exp((-x*x)/(2*sigma2));
            G->val[i][j] *= sf*exp((-y*y)/(2*sigma2));
            total += G->val[i][j];
        }
    }
    G = Matrix_scaleI(G,1/total); // normalize gaussian
    return G;
}

Matrix* Matrix_createIdentity(int rows, int columns) {
    Matrix *m = Matrix_createE(rows,columns);
    int i, j;
    for (i = 0; i < m->rows; i++) {
        for (j = 0; j < m->columns; j++) {
            if (i == j) {
                m->val[i][j] = 1;
            } else {
                m->val[i][j] = 0;
            }
        }
    }
    return m;
}

// returns rbt about the z axis 3D homogeneous
Matrix* Matrix_rotateZ(float theta) {
    Matrix *R = Matrix_createIdentity(4, 4);
    
    R->val[0][0] = cosf(theta);
    R->val[0][1] = -sinf(theta);
    R->val[1][0] = -(R->val[0][1]);
    R->val[1][1] = R->val[0][0];
    
    return R;
}

Matrix* Matrix_rotateX(float theta) {
    Matrix *R = Matrix_createIdentity(4, 4);
    
    R->val[1][1] = cosf(theta);
    R->val[1][2] = -sinf(theta);
    R->val[2][1] = -(R->val[1][2]);
    R->val[2][2] = R->val[1][1];
    
    return R;

}

Matrix* Matrix_rotateY(float theta) {
    Matrix *R = Matrix_createIdentity(4, 4);
    
    R->val[0][0] = cosf(theta);
    R->val[0][2] = sinf(theta);
    R->val[2][0] = -(R->val[0][2]);
    R->val[2][2] = R->val[0][0];
    
    return R;
}

Matrix* Matrix_translateX(float x) {
    Matrix *R = Matrix_createIdentity(4, 4);

    R->val[0][3] = x;
    return R;
}

Matrix* Matrix_translateY(float y) {
    Matrix *R = Matrix_createIdentity(4, 4);
    
    R->val[1][3] = y;
    return R;
}

Matrix* Matrix_translate(float x, float y, float z) {
    Matrix *R = Matrix_createIdentity(4, 4);
    R->val[0][3] = x;
    R->val[1][3] = y;
    R->val[2][3] = z;
    return R;
}

Matrix* Matrix_translateZ(float z) {
    Matrix *R = Matrix_createIdentity(4, 4);
    
    R->val[2][3] = z;
    return R;
}

Matrix* Matrix_createMan(float **values, int rows, int columns) {
    Matrix *m = Matrix_createE(rows, columns);
    m->val = values;
    m->mode = FALSE;
    return m;
}

Matrix* Matrix_createZeros(int rows, int columns) {
    Matrix *m = Matrix_createE(rows,columns);
    int i, j;
    for (i = 0; i < m->rows; i++) {
        for (j = 0; j < m->columns; j++) {
            m->val[i][j] = 0;
        }
    }
    return m;
}

void Matrix_destroy(Matrix *m) {
    if (m->mode) {
        //float **val = m->val;
        free((m->val[0])); // free the one big block of floats
        free(m->val); // free the array of pointers
    }
    free(m);
}

// determinant of a 2x2 or 3x3 matrix
float Matrix_det(Matrix *m) {
    // matrix must be square
    if (m->rows != m->columns) {
        printf("Warning: expected square matrix: Matrix_det\n");
        exit(1);
    }
    // only support 2x2 or 3x3 for the moment
    if (!((m->rows == 2) || (m->rows == 3))) {
        printf("Warning: only support 2x2 or 3x3 matrix: Matrix_det\n");
        exit(1);
    }
    float ** A = m->val;
    if (m->rows == 2) {
        return A[0][0] * A[1][1] - A[0][1] * A[1][0];
    } else {
        return  (A[0][0] * (A[2][2] * A[1][1] - A[2][1] * A[1][2]) - A[1][0] * (A[2][2] * A[0][1] - A[2][1] * A[0][2]) + A[2][0] * (A[1][2] * A[0][1] - A[1][1] * A[0][2]));  
    }
}

void Matrix_eigDecomp(Matrix *m, Matrix **ans) {
    if ((m->rows != m->columns) || (m->rows != 2)) {
        printf("Warning: only supports 2x2 matrix: Matrix_eigDecomp\n");
        exit(1);
    }
    
    float **A = m->val;
    float T = A[0][0] + A[1][1];
    float D = Matrix_det(m);
    
    Matrix *mLambda = Matrix_createE(2,2); // will store eigenvalues
    Vec *vQ0 = Vec_createE(2); // 1st eignvector
    Vec *vQ1 = Vec_createE(2); // 2nd
    
    float **Lambda = mLambda->val;
    float *Q0 = vQ0->val;
    float *Q1 = vQ1->val;
    
    // compute eigenvalues and initialize other elements
    Lambda[0][0] = T/2 + sqrt((T*T/4)-D); // this one is guarenteed to be the bigger one
    Lambda[0][1] = 0;
    Lambda[1][1] = T/2 - sqrt((T*T/4)-D);
    Lambda[1][0] = 0;

    // compute eigenvectors
    if (A[1][0] != 0) {
        // first eigen vector
        Q0[0] = Lambda[0][0] - A[1][1];
        Q0[1] = A[1][0];
        // second eigen vector
        Q1[0] = Lambda[1][1] - A[1][1];
        Q1[1] = A[1][0];
    } else if (A[0][1] != 0) {
        // first eigen vector
        Q0[0] = A[0][1];
        Q0[1] = Lambda[0][0] - A[0][0];
        // second eigen vector
        Q1[0] = A[0][1];
        Q1[1] = Lambda[1][1] - A[1][1];
    } else if (A[1][1] != 0) {
        // first eigen vector
        Q0[0] = 0;
        Q0[1] = 1;
        // second eigen vector
        Q1[0] = 1;
        Q1[1] = 0;
    } else {
        // first eigen vector
        Q0[0] = 1;
        Q0[1] = 0;
        // second eigen vector
        Q1[0] = 0;
        Q1[1] = 1;
    }

    // normalize eigenvectors which are down the columns
    Vec_normalizeI(vQ0);
    Vec_normalizeI(vQ1);
    
    // compose vectors into a matrix
    Matrix *mQ = Matrix_createE(2,2);
    float **Q = mQ->val;
    Q[0][0] = Q0[0];
    Q[1][0] = Q0[1];
    Q[0][1] = Q1[0];
    Q[1][1] = Q1[1];
    
    // put our answers in the given vector
    ans[0] = mLambda;
    ans[1] = mQ;
}

short Matrix_equals(Matrix *A, Matrix *B) {
    // check dimensions
    if ((A == NULL) || (B == NULL) || (A->rows != B->rows) || (A->columns != B->columns)) {
        return FALSE;
    }
    
    int i,j;
    for (i = 0; i < A->rows; i++) {
        for (j = 0; j < A->columns; j++) {
            if (A->val[i][j] != B->val[i][j]) {
                return FALSE;
            }
        }
    }
    
    return TRUE;
}

// should be pretty quick
Vec* Matrix_getRow(Matrix *m,int row) {
    Vec* v = Vec_createE(m->columns);
    memcpy(v->val,m->val[row],(sizeof(float) * (m->columns)));
    v->isColumn = FALSE;
    return v;
}

// maybe not so quick
Vec* Matrix_getColumn(Matrix *m, int column) {
    Vec *v = Vec_createE(m->rows);
    int i;
    for (i = 0; i < m->rows; i++) {
        v->val[i] = m->val[i][column];
    }
    return v;
}

// supports 2x2 and 3x3 inverses
Matrix* Matrix_inverse(Matrix *m) {
    
    float det = Matrix_det(m);
    if (fabsf(det) <= 10e-16) {
        printf("Warning: Matrix may be singular: det = %f \n",det);
    }
    
    Matrix *Inv = Matrix_createE(m->rows,m->rows);
    float **A = m->val;
    float **AI = Inv->val;
    // 2x2 case
    if (m->rows == 2) {
        AI[0][0] = A[1][1];
        AI[0][1] = -A[0][1];
        AI[1][0] = -A[1][0];
        AI[1][1] = A[0][0];
        return Matrix_scaleI(Inv, 1/det);
    } else {
        AI[0][0] = A[2][2] * A[1][1] - A[2][1] * A[1][2];
        AI[0][1] = -(A[2][2] * A[0][1] - A[2][1] * A[0][2]);
        AI[0][2] = A[1][2] * A[0][1] - A[1][1] * A[0][2];
        AI[1][0] = -(A[2][2] * A[1][0] - A[2][0] * A[1][2]);
        AI[1][1] = A[2][2] * A[0][0] - A[2][0] * A[0][2];
        AI[1][2] = -(A[1][2] * A[0][0] - A[1][0] * A[0][2]);
        AI[2][0] = A[2][1] * A[1][0] - A[2][0] * A[1][1];
        AI[2][1] = -(A[2][1] * A[0][0] - A[2][0] * A[0][1]);
        AI[2][2] = A[1][1] * A[0][0] - A[1][0] * A[0][1];
        return Matrix_scaleI(Inv, 1/det);
    }
}

Matrix* Matrix_multiplyAB(Matrix *A, Matrix *B) {
    // check inner Dimensions
    if (A->columns != B->rows) {
        printf("Warning incompatible dimensions!: Matrix_multiplyAB\n");
        exit(1);
    }
    
    Matrix *C = Matrix_createE(A->rows,B->columns);
    int i,j,k;
    float sum;
    for (i = 0; i < C->rows; i++) {
        for (j = 0; j < C->columns; j++) {
            sum = 0;
            for (k = 0; k < A->columns; k++) {
                sum += (A->val[i][k])*(B->val[k][j]); 
            }
            C->val[i][j] = sum;
        }
    }
    return C;
}

Matrix* Matrix_multiplyABD(Matrix *A, Matrix *B) {
    Matrix *ans = Matrix_multiplyAB(A, B);
    Matrix_destroy(A);
    return ans;
}

Matrix* Matrix_multiplyAB_D(Matrix *A, Matrix *B) {
    Matrix *ans = Matrix_multiplyAB(A, B);
    Matrix_destroy(B);
    return ans;
}

Matrix* Matrix_multiplyABDD(Matrix *A, Matrix *B) {
    Matrix *ans = Matrix_multiplyAB(A, B);
    Matrix_destroy(A);
    Matrix_destroy(B);
    return ans;
}

Vec* Matrix_multiplyAV(Matrix *A, Vec *v) {
    // check inner dimensions
    if ((!(v->isColumn) && ((A->rows != 1) || (A->columns != 1))) || (A->columns != v->size)) {
        printf("Warning incompatible dimensions: Matrix_multiplyAV\n");
        exit(1);
    }
    
    Vec *ans = Vec_createE(A->rows);
    int i,k;
    float sum;
    for (i = 0; i < ans->size; i++) {
        sum = 0;
        for (k = 0; k < A->columns; k++) {
            sum += (A->val[i][k])*(v->val[k]);
        }
        ans->val[i] = sum;
    }
    return ans;
}

Vec* Matrix_multiplyAVD(Matrix *A, Vec *v) {
    Vec* ans = Matrix_multiplyAV(A, v);
    Matrix_destroy(A);
    return ans;
}

Vec* Matrix_multiplyAV_D(Matrix *A, Vec *v) {
    Vec* ans = Matrix_multiplyAV(A, v);
    Vec_destroy(v);
    return ans;
}

Vec* Matrix_multiplyAVDD(Matrix *A, Vec *v) {
    Vec* ans = Matrix_multiplyAV(A, v);
    Matrix_destroy(A);
    Vec_destroy(v);
    return ans;
}


Vec* Matrix_multiplyVB(Vec *v, Matrix *B) {
    // check inner dimensions
    if ((v->isColumn && ((B->rows != 1) || (B->columns != 1))) || (v->size != B->rows)) {
        printf("Warning incompatible dimensions: Matrix_multiplyVB\n");
        exit(1);
    }
    
    Vec *ans = Vec_createE(B->columns);
    ans->isColumn = FALSE; // its a row vector!
    int j,k;
    float sum;
    for (j = 0; j < ans->size; j++) {
        sum = 0;
        for (k = 0; k < B->rows; k++) {
            sum += (v->val[k])*(B->val[k][j]);
        }
        ans->val[j] = sum;
    }
    return ans;
}

Vec* Matrix_multiplyVBD(Vec *v, Matrix *B) {
    Vec* ans = Matrix_multiplyVB(v,B);
    Vec_destroy(v);
    return ans;
}

Vec* Matrix_multiplyVB_D(Vec *v, Matrix *B) {
    Vec* ans = Matrix_multiplyVB(v,B);
    Matrix_destroy(B);
    return ans;
}

Vec* Matrix_multiplyVBDD(Vec *v, Matrix *B) {
    Vec* ans = Matrix_multiplyVB(v,B);
    Vec_destroy(v);
    Matrix_destroy(B);
    return ans;
}

void Matrix_print(Matrix *m) {
    
    // 15 is the minimal size needed to hold the buffer with the below formatting specifications
    char full[((m->columns)*15 + 1) * (m->rows)];
    char buffer[15];
    strcpy(full,""); // need a null termination to make strcat happy
    int i,j;
    float f;
    for (i = 0; i < m->rows; i++) {
        for (j = 0; j < m->columns; j++) {
            f = m->val[i][j];
            if (f >= 1000000) {
                sprintf(buffer,"%.5e ",f);
            } else {
                sprintf(buffer,"%.5f ",f);
            }
            strcat(full,buffer);
        }
        if (i != (m->rows - 1)) {
            strcat(full,"\n");
        }
    }
    printf("[ %s]\n",full);
}

Matrix* Matrix_scale(Matrix *m, float scaleFactor) {
    Matrix *ans = Matrix_copy(m);
    return Matrix_scaleI(ans,scaleFactor);
}

Matrix* Matrix_scaleI(Matrix *target, float scaleFactor) {
    int i,j;
    for (i = 0; i < target->rows; i++) {
        for (j = 0; j < target->columns; j++) {
            target->val[i][j] *= scaleFactor;
        }
    }
    return target;
}

Matrix* Matrix_subtract(Matrix *A, Matrix *B) {
    Matrix *C = Matrix_copy(A);
    return Matrix_subtractI(C,B);
}
Matrix* Matrix_subtractI(Matrix *target, Matrix *B) {
    if ((target->rows != B->rows) || (target->columns != B->columns)) {
        printf("Warning: Incompatible dimensions: Matrix_addI\n");
        exit(1);
    }
    
    int i,j;
    for (i = 0; i < target->rows; i++) {
        for (j = 0; j < target->columns; j++) {
            target->val[i][j] -= B->val[i][j];
        }
    }
    return target;
}

Matrix* Matrix_subtractID(Matrix *target, Matrix *B) {
    Matrix_subtractI(target, B);
    Matrix_destroy(B);
    return target;
}

Matrix* Matrix_subtractD(Matrix *A, Matrix *B) {
    Matrix *ans = Matrix_subtract(A, B);
    Matrix_destroy(A);
    return ans;
}

Matrix* Matrix_subtract_D(Matrix *A, Matrix *B) {
    Matrix *ans = Matrix_subtract(A, B);
    Matrix_destroy(B);
    return ans;
}

Matrix* Matrix_subtractDD(Matrix *A, Matrix *B) {
    Matrix *ans = Matrix_subtract(A, B);
    Matrix_destroy(A);
    Matrix_destroy(B);
    return ans;
}

Matrix* Matrix_transpose(Matrix *m) {
    Matrix *mt = Matrix_createE(m->columns, m->rows);
    int i,j;
    for (i = 0; i < m->rows; i++) {
        for (j = 0; j < m->columns; j++) {
            mt->val[j][i] = m->val[i][j];
        }
    }
    return mt;
}

/*********************************************************/


void VecList_add(VecList *vlist, Vec *v) {
    // if still room to spare
    if (vlist->size < vlist->maxsize) {
        vlist->list[vlist->size] = v;
        (vlist->size)++;
    } 
    // need to expand the list
    else {
        vlist->list = (Vec **) realloc(vlist->list,sizeof(Vec *)*(vlist->maxsize) * 2);
        if (vlist->list == NULL) {
            printf("Warning: realloc failed: Vec_list_add\n");
            exit(1);
        }
        vlist->maxsize = (vlist->maxsize)*2;
        // add the new point
        vlist->list[vlist->size] = v;
        (vlist->size)++;
    }
}

// appends the shorter arraylist to the longer one and then destroys the short one
// and fixes pointers
void VecList_combineI(VecList **targetptr, VecList **blistptr) {
    
    // rearange pointers if neccessary
    if ((*blistptr)->size > (*targetptr)->size) {
        VecList *temp = *blistptr;
        *blistptr = *targetptr;
        *targetptr = temp;
    }
    
    VecList *target = *targetptr;
    VecList *blist = *blistptr;
    
    target->maxsize = (target->size + blist->size) * 2;
    // expand target
    target->list = (Vec **) realloc(target->list,sizeof(Vec *) * (target->maxsize));
    if (target->list == NULL) {
        printf("Warning: realloc failed: Vec_list_combineI\n");
        exit(1);
    }
    
    // copy information over
    memcpy(&(target->list[target->size]),blist->list, sizeof(Vec *) * blist->size);
    target->size += blist->size;
    
    // destroy blist
    VecList_destroy(blist);
    return;
}

// copies the structure of the list, but doesn't make copies of the contained verticies
VecList* VecList_copy(VecList *that) {
    VecList* this = VecList_createS((that->size)*2);
    
    memcpy(this->list,that->list,(sizeof(Vec *) * (that->size)));
    
    this->size = that->size;
    return this;
}

VecList* VecList_create(void) {
    return VecList_createS(BASESIZE);
}

VecList* VecList_createS(int initSize) {
    if (initSize <= 0) {
        printf("Warning: invalid initial size, setting to default: Vec_list_createS\n");
        initSize = BASESIZE;
    }
    VecList *vlist = (VecList *) malloc(sizeof(VecList));
    // allocate size for the structure
    if (vlist == NULL) {
        printf("Warning: malloc failed: Vec_list_createI:1\n");
        exit(1);
    }
    
    vlist->size = 0;
    vlist->maxsize = initSize;
    
    // allocate space for its list of vector pointers
    vlist->list = (Vec **) malloc(sizeof(Vec *) * initSize);
    if (vlist->list == NULL) {
        printf("Warning: malloc failed: Vec_list_createI:2\n");
        exit(1);
    }
    return vlist;
}

// destroys only this structure
void VecList_destroy(VecList *vlist) {
    free(vlist->list);
    free(vlist);
}

// destroys all contained vectors and this structure
void VecList_destroyR(VecList *vlist) {
    int i;
    for (i = 0; i < vlist->size; i++) {
        Vec_destroy(vlist->list[i]);
    }
    free(vlist->list);
    free(vlist);
}

// shows how to get a vector from the list
Vec* V_getV(VecList *vlist, int index) {
    if (index >= vlist->size) {
        printf("Warning: index out of bounds: V_get index = %d, size = %d \n",index,vlist->size);
        exit(1);
    } else if (index < 0) {
        printf("Warning: invalid index: V_get index = %d \n",index);
        exit(1);
    }
    return vlist->list[index];
}

// removes vector from vector list without deleting it
short VecList_remove(VecList *vlist, int index) {
    if ((index >= vlist->size) || (index < 0)) {
        return FALSE;
    } 
    // if its on the end, we can get rid of it really quick
    else if (index == ((vlist->size) - 1)) {
        (vlist->size)--;
    } 
    // otherwise shift everthing over to the left
    else {
        memcpy(&(vlist->list[index]),&(vlist->list[index + 1]),sizeof(Vec *) * ((vlist->size) - (index + 1)));
        (vlist->size)--;
    }
    return TRUE;
}

// removes vector at index and deletes it
short VecList_removeR(VecList *vlist, int index) {
    if ((index >= vlist->size) || (index < 0)) {
        return FALSE;
    } 
    // if its on the end, we can get rid of it really quick
    else if (index == ((vlist->size) - 1)) {
        Vec_destroy(vlist->list[index]);
        (vlist->size)--;
    } 
    // otherwise shift everthing over to the left
    else {
        Vec_destroy(vlist->list[index]);
        memcpy(vlist->list[index],vlist->list[index + 1],sizeof(Vec *) * ((vlist->size) - 1 - (index + 1)));
    }
    return TRUE;
}

